import lejos.nxt.*;
import lejos.nxt.comm.*;
import lejos.navigation.Pilot;
import java.io.*;
/**
 *
 * @author Alex Sanches, Flavio Pacheco
 */

/*
 int tempoViagem = 1000*Math.round( dist / ( ( pilot.getSpeed() / voltaInteira ) * distVolta ) );
				
		try
		{
			Thread.sleep(tempoViagem);
	    }
	    catch(InterruptedException e)
	    {	    	
	    }
*/
public class LICReceiverIV 
{
	static final float fatorConversaoComprimento = 1/2.54f;
	static final int voltaInteira = 360;
	static final float comprimentoRoda = (float) Math.PI * 5.6f*fatorConversaoComprimento;
	static final float deixos = 10.4f*fatorConversaoComprimento;
	static final float raioCurvatura = 17*fatorConversaoComprimento; 
	
	private static void ResetAllMotors()
	{
		ResetMotor(Motor.A);
		ResetMotor(Motor.B);
	}
	
	private static void ResetMotor(Motor m)
	{
		m.resetTachoCount();
	}
	
	private static void StopAllMotors()
	{
		StopMotor(Motor.A);
		StopMotor(Motor.B);
	}
	
	private static void StopMotor(Motor m)
	{
		m.stop();
		m.stop();
	}
	
	private static void SpeedAllMotors(int s)
	{
		SpeedMotor(Motor.A, s);
		SpeedMotor(Motor.B, s);
	}
	
	private static void SpeedMotor(Motor m, int s)
	{
		m.setSpeed(s);
	}
	
	private static void FwdAllMotors()
	{
		FwdMotor(Motor.A);
		FwdMotor(Motor.B);
	}
	
	private static void FwdMotor(Motor m)
	{
		m.forward();
	}
	
	private static void BckAllMotors()
	{
		BckMotor(Motor.A);
		BckMotor(Motor.B);
	}
	
	private static void BckMotor(Motor m)
	{
		m.backward();
	}
	
	public static float Andar(Pilot pilot, float dist)
	{	
		boolean chegouA = false;
		boolean chegouB = false;
		
		dist *= fatorConversaoComprimento;
		dist /= comprimentoRoda/360;
		
		ResetAllMotors();
		FwdAllMotors();
		
		if(dist <= 0)
		{
			return 0;
		}
		
		while(!chegouA || !chegouB)
		{
			if(Math.abs(Motor.A.getTachoCount()) >= dist)
			{
				StopMotor(Motor.A);
				chegouA = true;
			}
			
			if(Math.abs(Motor.B.getTachoCount()) >= dist)
			{
				StopMotor(Motor.B);
				chegouB = true;
			}			
		}
		
		StopAllMotors();
       
        float d = pilot.getTravelDistance();
        d /= fatorConversaoComprimento;
        return d;
	}
	
	public static float Voltar(Pilot pilot, float dist)
	{
		boolean chegouA = false;
		boolean chegouB = false;
		
		dist *= fatorConversaoComprimento;
		dist /= comprimentoRoda/360;
		
		ResetAllMotors();
		BckAllMotors();
		
		if(dist <= 0)
		{
			return 0;
		}
		
		while(!chegouA || !chegouB)
		{
			if(Math.abs(Motor.A.getTachoCount()) >= dist)
			{
				StopMotor(Motor.A);
				chegouA = true;
			}
			
			if(Math.abs(Motor.B.getTachoCount()) >= dist)
			{
				StopMotor(Motor.B);
				chegouB = true;
			}			
		}
		
		StopAllMotors();
        
        float d = pilot.getTravelDistance();
        d /= fatorConversaoComprimento;
        return d;
	}
	
	public static float Parar(Pilot pilot, float dist)
	{
		boolean chegouA = false;
		boolean chegouB = false;
		
		dist *= fatorConversaoComprimento;
		dist /= comprimentoRoda/360;
		
		ResetAllMotors();
		
		while(!chegouA || !chegouB)
		{
			if(Math.abs(Motor.A.getTachoCount()) >= dist)
			{
				StopMotor(Motor.A);
				chegouA = true;
			}
			
			if(Math.abs(Motor.B.getTachoCount()) >= dist)
			{
				StopMotor(Motor.B);
				chegouB = true;
			}			
		}
		
		StopAllMotors();
        
        float d = pilot.getTravelDistance();
        d /= fatorConversaoComprimento;
        return d;
	}
	
	public static void Velocidade(int v)
	{
		if(v > 110*Battery.getVoltage())
		{
			v = Math.round(110*Battery.getVoltage());
		}
		
		SpeedAllMotors(v);
	}
	
	public static float Girar(int grau)
	{
		boolean chegouA = false;
		boolean chegouB = false;
		
		ResetAllMotors();		
		
		float dist = (deixos/2)*((grau) / (180 / (float)Math.PI));
		
		dist /= comprimentoRoda/360;
		
		dist = (float)Math.abs(dist);		
		
		if(grau > 0)
		{
			BckMotor(Motor.A);
			FwdMotor(Motor.B);
		}
		else if (grau < 0)
		{
			FwdMotor(Motor.A);
			BckMotor(Motor.B);			
		}
		
		while(!chegouA || !chegouB)
		{
			if(Math.abs(Motor.A.getTachoCount()) >= dist)
			{
				StopMotor(Motor.A);
				chegouA = true;
			}
			
			if(Math.abs(Motor.B.getTachoCount()) >= dist)
			{
				StopMotor(Motor.B);
				chegouB = true;
			}			
		}
		
		//return (Math.abs(Motor.A.getTachoCount())/distVolta)/360;
		
		/* Math.abs (Motor.A.getTachoCount ()) * (distVolta / 360): Esse termo calcula o comprimento do arco descrito pela roda para percorrer o angulo Math.abs (Motor.A.getTachoCount ()) */
		/* ((Math.abs (Motor.A.getTachoCount ()) * (distVolta / 360)) / (deixos / 2)): Esse termo calcula o angulo, em radianos, do arco descrito pela roda (quanto o robo girou em radianos) */
		/* ((Math.abs (Motor.A.getTachoCount ()) * (distVolta / 360)) / (deixos / 2)) * (180 / (float) Math.PI): Esse termo converte o angulo de giro do robo de radianos para graus */	
		return ((Math.abs(Motor.A.getTachoCount ()) * (comprimentoRoda / 360)) / (deixos / 2)) * (180 / (float) Math.PI); // Retorna o angulo descrito pelo giro do robo em graus
		//versao simplificada da mesma formula:
		//faz a proporcao entre o angulo de giro da roda, o raio da roda e o raio do eixo.
		//return Math.abs(Motor.A.getTachoCount()) * 5.6f/10.4f;
	}
	
	public static void AndaMotor(Motor m1, float d1, Motor m2, float d2)
	{
		boolean chegou1 = false;
		boolean chegou2 = false;
		boolean andando = m1.isMoving();
				
		ResetAllMotors();
		
		int bkpV = m1.getSpeed();
		
		if(andando)
		{
			StopAllMotors();
		}
		
		if(d1 > d2)
		{
			m2.setSpeed(Math.round(bkpV*((raioCurvatura-deixos/2)/(raioCurvatura+deixos/2))));
			FwdMotor(m2);
			FwdMotor(m1);
		}
		else
		{
			m1.setSpeed(Math.round(bkpV*((raioCurvatura-deixos/2)/(raioCurvatura+deixos/2))));
			FwdMotor(m1);
			FwdMotor(m2);
		}		
		
		while(!chegou1 || !chegou2)
		{
			if(Math.abs(m1.getTachoCount()) >= d1)
			{
				StopMotor(m1);				
				chegou1 = true;
			}
			
			if(Math.abs(m2.getTachoCount()) >= d2)
			{
				StopMotor(m2);
				chegou2 = true;
			}			
		}
		
		Velocidade(bkpV);
		
		if(andando)
		{
			FwdAllMotors();
		}
	}
	
	public static void Esquerda(int grau)
	{
		float distA,distB;
		distA = (raioCurvatura - deixos/2)*((grau) / (180 / (float)Math.PI));
		distA /= comprimentoRoda/360;
		
		distB = (raioCurvatura + deixos/2)*((grau) / (180 / (float)Math.PI));
		distB /= comprimentoRoda/360;
		
		AndaMotor(Motor.A, distA, Motor.B, distB);
	}
	
	public static void Direita(int grau)
	{
		float distA,distB;
		distA = (raioCurvatura + deixos/2)*((grau) / (180 / (float)Math.PI));
		distA /= comprimentoRoda/360;
		
		distB = (raioCurvatura - deixos/2)*((grau) / (180 / (float)Math.PI));
		distB /= comprimentoRoda/360;
		
		AndaMotor(Motor.A, distA, Motor.B, distB);		
	}
	
	public static void Resposta(DataOutputStream dos, float and, float rot) throws IOException
	{		
		dos.writeFloat(and);     
        dos.writeFloat(rot);      
        dos.flush();
	}
	
    public static void main(String [] args)  throws Exception 
    { 
    	int parm = 0;
    	float rot = 0;
    	float and = 0;
    	boolean conectado = false;
    	boolean waitingConnection = true;
    	DataInputStream dis = null; 
    	DataOutputStream dos = null;
    	// Cria vetor que armazenara as mensagens passadas ao NXT
    	char [] message = new char[10];
    	Pilot pilot = null;
    	UltrasonicSensor ult = null;
    	BTConnection btc = null;
        
		// Strings das mensagens padrao do LCD
        String connected = "Connected";
        String waiting = "Waiting...";
        String closing = "Closing...";
    	
        // Repete ate o botao "ESCAPE" do NXT ser pressionado
        while (!Button.ESCAPE.isPressed() && (conectado || waitingConnection))
        {
        	if (waitingConnection) {
                
                //Sensor de ultrason
                ult = new UltrasonicSensor(SensorPort.S4);
                
                // Escreve no LCD a mensagem em waiting        
                LCD.drawString(waiting,0,0);
                LCD.refresh();
                
                // Aguarda a conexao bluetooth - BLOQUEANTE
                btc = Bluetooth.waitForConnection();
                conectado = true;

                // Escreve no LCD a mensagem em connected
                LCD.clear();
                LCD.drawString(connected,0,0);
                LCD.refresh();	
                
                pilot = new Pilot(5.6f*fatorConversaoComprimento, 9.7f*fatorConversaoComprimento, Motor.A, Motor.B); //units: inches
                
                // Abre stream de dados de entrada e saida
                dis = btc.openDataInputStream();
                dos = btc.openDataOutputStream();
                
                // Escreve "Sist. Pronto" no LCD
                LCD.drawString("Sist. Pronto", 0, 0);
                
                waitingConnection = false;
			}
        	
            int msgcount=0; // Contador de elementos do vetor de mensagem
            char c; // Variavel temporaria para leitura dos bytes da mensagem
            
            // Le a mensagem enviada com no maximo 10 caracteres
            do{
                c = (char) dis.readByte(); // Le um byte
                message[msgcount]=(char)c; // Adiciona ao vetor
                msgcount++;
            }while (msgcount<10 && c!='\n'); // Testa se chegou a 10 ou a mensagem acabou
            
            // Passa a mensagem para a string 'cmd' 
            String cmd ="";
            for (int i=0; i< message.length; i++)
                cmd += message[i];
            
            // Escreve no LCD "cmd: " e o conteudo de cmd; escreve "cmd size: " e tamanho de cmd             
            LCD.drawString("cmd: " + cmd, 0, 2);
            LCD.drawString("cmd size: " + cmd.length(), 0, 3);
            
            // Testa para verificar qual e o comando
            if (cmd.charAt(0)=='S' && cmd.charAt(1)=='T' && cmd.charAt(2)=='P')
            {          
                LCD.clear();
                LCD.drawString("PARANDO AGORA", 0, 1);
                
                and = Parar(pilot, 0);
                LCD.drawString("PAROU EM " + Math.round(and), 0, 2);
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='M' && cmd.charAt(1)=='E' && cmd.charAt(2)=='D') {
            	int distancia = ult.getDistance();
            	LCD.clear();
            	LCD.drawString("Entrei", 0, 0);
            	LCD.drawInt(distancia, 0, 2);
            	dos.writeInt(distancia);
            	dos.flush();
            	continue;
			}
            
            if (cmd.charAt(0)=='P' && cmd.charAt(1)=='A' && cmd.charAt(2)=='R')
            {
            	parm = dis.readInt();
                LCD.clear();
                LCD.drawString("PARANDO EM " + parm, 0, 1);
                
                and = Parar(pilot, parm);
                LCD.drawString("PAROU EM " + Math.round(and), 0, 2);
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='B' && cmd.charAt(1)=='C' && cmd.charAt(2)=='K')
            {
                LCD.clear();
                LCD.drawString("VOLTANDO", 0, 1);
                
                and = Voltar(pilot, 0);
                LCD.drawString("VOLTOU " + Math.round(and), 0, 2);
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='V' && cmd.charAt(1)=='O' && cmd.charAt(2)=='L')
            { 
            	parm = dis.readInt();
                LCD.clear();
                LCD.drawString("VOLTANDO " + parm, 0, 1);
                
                and = Voltar(pilot, parm);
                LCD.drawString("VOLTOU " + Math.round(and), 0, 2);
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='F' && cmd.charAt(1)=='W' && cmd.charAt(2)=='D')
            { 
            	LCD.clear();
                LCD.drawString("ANDANDO", 0, 1);
                
                and = Andar(pilot, 0);
                LCD.drawString("ANDOU " + Math.round(and), 0, 2);                
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='A' && cmd.charAt(1)=='N' && cmd.charAt(2)=='D')
            { 
            	parm = dis.readInt();
            	LCD.clear();
                LCD.drawString("ANDANDO " + parm, 0, 1);
                
                and = Andar(pilot, parm);
                LCD.drawString("ANDOU " + Math.round(and), 0, 2);
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='G' && cmd.charAt(1)=='I' && cmd.charAt(2)=='R')
            { 
            	parm = dis.readInt();
            	LCD.clear();
                LCD.drawString("GIRANDO " + parm, 0, 1);
                
                rot = Girar(parm);
                LCD.drawString("GIROU " + Math.round(rot), 0, 2);
                and = 0;
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='E' && cmd.charAt(1)=='S' && cmd.charAt(2)=='Q')
            { 
            	parm = dis.readInt();
            	LCD.clear();
                LCD.drawString("VIRANDO ESQ", 0, 1);
                
                Esquerda(parm);
                and = pilot.getTravelDistance()/fatorConversaoComprimento;
                rot = pilot.getAngle();
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='D' && cmd.charAt(1)=='I' && cmd.charAt(2)=='R')
            { 
            	parm = dis.readInt();
            	LCD.clear();
                LCD.drawString("VIRANDO DIR", 0, 1);
                
                Direita(parm);
                and = pilot.getTravelDistance()/fatorConversaoComprimento;
                rot = pilot.getAngle();
                Resposta(dos,and,rot);
                continue;
            }
            
            if (cmd.charAt(0)=='V' && cmd.charAt(1)=='E' && cmd.charAt(2)=='L')
            { 
            	parm = dis.readInt();
            	LCD.clear();
                LCD.drawString("VELOCIDADE " + parm, 0, 1);
                
                Velocidade(parm);
                continue;
            }
            
            if (cmd.charAt(0)=='E' && cmd.charAt(1)=='N' && cmd.charAt(2)=='D')
            {             	
            	LCD.clear();
                LCD.drawString("DESCONECTANDO", 0, 1);
                conectado = false;
            }
            if (cmd.charAt(0)=='D' && cmd.charAt(1)=='I' && cmd.charAt(2)=='S') {
				conectado = false;
				waitingConnection = true;
			}
        }
        dis.close();
        dos.close();
        Thread.sleep(100); // wait for data to drain
        LCD.clear();
        LCD.drawString(closing,0,0);
        LCD.refresh();
        btc.close();
        LCD.clear();
    }
}
